#include <stdio.h>
#include <unistd.h>
#include <fcntl.h>
#include "control.h"

#include "communicate.h"


const int8_t jdq1_on[8]  = {0xFE,0x05,0x00,0x00,0xFF,0x00,0x98,0x35};
const int8_t jdq1_off[8] = {0xFE,0x05,0x00,0x00,0x00,0x00,0xD9,0xC5};
const int8_t jdq2_on[8]  = {0xFE,0x05,0x00,0x01,0xFF,0x00,0xC9,0xF5};
const int8_t jdq2_off[8] = {0xFE,0x05,0x00,0x01,0x00,0x00,0x88,0x05};
const int8_t jdq3_on[8]  = {0xFE,0x05,0x00,0x02,0xFF,0x00,0x39,0xF5};
const int8_t jdq3_off[8] = {0xFE,0x05,0x00,0x02,0x00,0x00,0x78,0x05};
const int8_t jdq4_on[8]  = {0xFE,0x05,0x00,0x03,0xFF,0x00,0x68,0x35};
const int8_t jdq4_off[8] = {0xFE,0x05,0x00,0x03,0x00,0x00,0x29,0xC5};

const int8_t py_stop[5] = "stop";
const int8_t py_run[4] = "run";
const int8_t py_front[6] = "front";
const int8_t py_behind[7] = "behind";

/**
 * 风扇控制
 * action:  
 *        1: 风扇全速 100%
 *        0: 40%  
 * */
static void nvidia_fan_ctrl(uint8_t action)
{
    int fd;
    int ret;
    
    fd = open("/sys/devices/pwm-fan/target_pwm",O_RDWR);
    if(fd < 0) {
        printf("Fan control failed: open\n");
        close(fd);
        return;
    }
    
    if(action) {    //打开风扇
        ret = write(fd, FAN_255, 3);    //nvidia风扇
    }
    else{
        ret = write(fd, FAN_100, 3);    //nvidia风扇
    }
    if(ret != 3){
        printf("Fan control failed: write\n");
    }
    close(fd);
}


/*
 * 车身散热风扇
 * action：1：打开  0：关闭
 * */
void four_fans_ctrl(uint8_t action)
{
    if(action == 1)
    {
        udp_cmd_package(232,10000,jdq4_on,8);   //电源：云台、导航灯 打开
    }
    else
    {
        udp_cmd_package(232,10000,jdq4_off,8);   //电源：云台、导航灯 打开
    }
}


/*
 * 导航灯控制
 * action：1：打开  0：关闭
 * */
void navigation_led_ctrl(uint8_t action)
{
    if(action == 1)
    {
        udp_cmd_package(232,10000,jdq3_on,8);   //电源：云台、导航灯 打开
    }
    else
    {
        udp_cmd_package(232,10000,jdq3_off,8);   //电源：云台、导航灯 打开
    }
}

/*
 * 云台电源控制
 * 包括：导航灯电源、激光雷达电源
 * action：1：打开  0：关闭
 * 云台电源常开接线方法
 * */
void power_yuntai_ctrl(uint8_t action)
{
    if(action == 1)
    {
        udp_cmd_package(232,10000,jdq1_off,8);   //电源：云台、导航灯 打开
    }
    else
    {
        udp_cmd_package(232,10000,jdq1_on,8);  //电源：云台、导航灯 关闭
    }
}


/*
 * 门控制
 * action: 1: 开门     0: 关门
 * */
void door_ctrl(uint8_t action)
{

}

/*
 * 读门状态
 * return: 0：关闭状态      1：打开状态
 * */

void door_sta_get(void)
{

    
}


/*
 * 机器人状态指示灯控制
 * sta: 0:绿灯，机器人正在运行
 *      1:黄灯，机器人位于充电房，正在充电
 *      2:红灯，机器人不正常、遇到障碍物、低电压、出线等
 * */
void status_leds_ctrl(uint8_t sta)
{
    switch (sta)
    {
        case GREEN:
            uart_cmd_package(UART_CMD_WRITE, ROBOT_STA_LEDS, GREEN);
            break;
        case YELLOW:
            uart_cmd_package(UART_CMD_WRITE, ROBOT_STA_LEDS, YELLOW);
            break;
        case RED:
            uart_cmd_package(UART_CMD_WRITE, ROBOT_STA_LEDS, RED);
            break;
        default:
            printf("status_leds_ctrl ERROR!\n");
            
    }

}


/*
 * 低功耗模式进入/退出
 * 云台激光雷达断电、python程序暂停、NVIDIA风扇速度减慢、状态灯变为黄色
 * action：     1:进入低功耗
 *              0:退出低功耗 
 * */
void low_power_ctrl(uint8_t action)
{
    if(action == 1) //进入低功耗
    {   
        power_yuntai_ctrl(0);  
        udp_cmd_package(102,9999,py_stop,4);    //导航识别程序关闭
        nvidia_fan_ctrl(0);                            //风扇关闭
        status_leds_ctrl(YELLOW);

    }
    else
    {
        power_yuntai_ctrl(1);
        udp_cmd_package(102,9999,py_run,3);     //导航识别程序打开
        nvidia_fan_ctrl(1);   
        status_leds_ctrl(GREEN);                //风扇打开
    }
    
}

/**
 * 导航摄像头切换
 * direction ： 1：前进方向
 *              0：后退方向
 * */
void navigation_camera_ctrl( uint8_t direction)
{
    if(direction == 1){
        udp_cmd_package(102,9999,py_front,5);
    }
    else if(direction == 0){
        udp_cmd_package(102,9999,py_behind,6);

    }
}


